#include <convert_manager.h>
#include <stdio.h>
#include <linux/videodev2.h>  
#include <stdlib.h>


static int Yuv2Rgb_isSupport(int InputPixelFormat, int OutputPixelFormat)
{
    if (iPixelFormatIn != V4L2_PIX_FMT_YUYV)
        return 0;
    if ((iPixelFormatOut != V4L2_PIX_FMT_RGB565) && (iPixelFormatOut != V4L2_PIX_FMT_RGB32))
    {
        return 0;
    }
    return 1;
}


// 将YUV422格式的视频帧数据转换为RGB565格式
// 参数: input_ptr-实际的YUV422视频帧数据;output_ptr-转换后的RGB565数据（需预先分配足够内存）
static unsigned int
Pyuv422torgb565(unsigned char * input_ptr, unsigned char * output_ptr, unsigned int image_width, unsigned int image_height)
{
	unsigned int i, size;
	unsigned char Y, Y1, U, V;
	unsigned char *buff = input_ptr;
	unsigned char *output_pt = output_ptr;

    unsigned int r, g, b;
    unsigned int color;
    
	size = image_width * image_height /2;
	for (i = size; i > 0; i--) {
		/* bgr instead rgb ?? */
		Y = buff[0] ;
		U = buff[1] ;
		Y1 = buff[2];
		V = buff[3];
		buff += 4;
		r = R_FROMYV(Y,V);
		g = G_FROMYUV(Y,U,V); //b
		b = B_FROMYU(Y,U); //v

        r = r >> 3;
        g = g >> 2;
        b = b >> 3;
        color = (r << 11) | (g << 5) | b;
        *output_pt++ = color & 0xff;
        *output_pt++ = (color >> 8) & 0xff;
			
		r = R_FROMYV(Y1,V);
		g = G_FROMYUV(Y1,U,V); //b
		b = B_FROMYU(Y1,U); //v
		
        r = r >> 3;
        g = g >> 2;
        b = b >> 3;
        color = (r << 11) | (g << 5) | b;
        *output_pt++ = color & 0xff;
        *output_pt++ = (color >> 8) & 0xff;
	}
	
	return 0;
} 


// 将YUV422格式的视频帧数据转换为RGB24格式
static unsigned int
Pyuv422torgb32(unsigned char * input_ptr, unsigned char * output_ptr, unsigned int image_width, unsigned int image_height)
{
	unsigned int i, size;
	unsigned char Y, Y1, U, V;
	unsigned char *buff = input_ptr;
	unsigned int *output_pt = output_ptr;

    unsigned int r, g, b;
    unsigned int color;

	size = image_width * image_height /2;
	for (i = size; i > 0; i--) {
		/* bgr instead rgb ?? */
		Y = buff[0] ;
		U = buff[1] ;
		Y1 = buff[2];
		V = buff[3];
		buff += 4;

        r = R_FROMYV(Y,V);
		g = G_FROMYUV(Y,U,V); //b
		b = B_FROMYU(Y,U); //v
		/* rgb888 */
		color = (r << 16) | (g << 8) | b;
        *output_pt++ = color;
			
		r = R_FROMYV(Y1,V);
		g = G_FROMYUV(Y1,U,V); //b
		b = B_FROMYU(Y1,U); //v
		color = (r << 16) | (g << 8) | b;
        *output_pt++ = color;
	}
	
	return 0;
} 


// 对ptVideoBuffIn的数据进行转换，保存到ptVideoBuffOut
static int Yuv2Rgb_Convert(PT_VideoBuff ptVideoBuffIn, PT_VideoBuff ptVideoBuffOut)
{
    PT_PixelDatas ptPixelDatasIn  = &ptVideoBuffIn->tPixelDatas;
    PT_PixelDatas ptPixelDatasOut = &ptVideoBuffOut->tPixelDatas;

    ptPixelDatasOut->iHeight = ptPixelDatasIn->iHeight;
    ptPixelDatasOut->iWidth = ptPixelDatasIn->iWidth;

    // 如果要转换成V4L2_PIX_FMT_RGB565
    if (ptVideoBufOut->iPixelFormat == V4L2_PIX_FMT_RGB565)
    {
        ptPixelDatasOut->iBpp = 16;
        ptPixelDatasOut->iLineBytes = ptVideoBuffOut->tPixelDatas.iWidth * ptVideoBuffOut->tPixelDatas.iBpp / 8;
        ptPixelDatasOut->iTotalBytes = ptVideoBuffOut->tPixelDatas.iLineBytes * ptVideoBuffOut->tPixelDatas.iHeight;
        // 如果输出缓冲区指针 aucPixelDatas 为空，则分配内存（后面Pyuv422torgb565要用到）
        if (!ptPixelDatasOut->aucPixelDatas)
        {
            ptPixelDatasOut->aucPixelDatas = malloc(ptPixelDatasOut->iTotalBytes);
        }
        Pyuv422torgb565(ptPixelDatasIn->aucPixelDatas, ptPixelDatasOut->aucPixelDatas, ptPixelDatasOut->iWidth, ptPixelDatasOut->iHeight);
    }
    // 如果要转换成V4L2_PIX_FMT_ARGB32
    else if (ptVideoBufOut->iPixelFormat == V4L2_PIX_FMT_ARGB32)
    {
        ptPixelDatasOut->iBpp = 32;
        ptPixelDatasOut->iLineBytes = ptVideoBuffOut->tPixelDatas.iWidth * ptVideoBuffOut->tPixelDatas.iBpp / 8;
        ptPixelDatasOut->iTotalBytes = ptVideoBuffOut->tPixelDatas.iLineBytes * ptVideoBuffOut->tPixelDatas.iHeight;
        // 空指针则分配内存
        if (!ptPixelDatasOut->aucPixelDatas)
        {
            ptPixelDatasOut->aucPixelDatas = malloc(ptPixelDatasOut->iTotalBytes);
        }
    }
    Pyuv422torgb32(ptPixelDatasIn->aucPixelDatas, ptPixelDatasOut->aucPixelDatas, ptPixelDatasOut->iWidth, ptPixelDatasOut->iHeight);
}


/**
 * @brief 释放YUV转RGB过程中分配的内存资源
 * @param outputBuff 指向视频缓冲区的结构体指针
 * @return 成功返回0，失败返回-1
 */
static int Yuv2Rgb_ConvertExit(PT_VideoBuff outputBuff)
{
    if (outputBuff == NULL) {
        return -1; // 错误：传入空指针
    }

    // 释放RGB数据缓冲区（如果已分配）
    if (outputBuff->tPixelDatas.aucPixelDatas != NULL) 
    {
        free(outputBuff->tPixelDatas.aucPixelDatas);
        outputBuff->tPixelDatas.aucPixelDatas = NULL; // 避免悬空指针（内存被释放但指针仍然指向原地址）
    }
    return 0; // 成功释放
}


static T_ConvertOpr g_tYuv2Rgb = {
    .name = "Yuv2Rgb",
    .isSupport = Yuv2Rgb_isSupport,
    .Convert = Yuv2Rgb_Convert,
    .ConvertExit = Yuv2Rgb_ConvertExit,
};


void RegisterYuv2Rgb(void)
{
    RegisterConvertOpr(&g_tYuv2Rgb);
}